﻿using System;
using System.Collections.Generic;

namespace TrueSync.Physics2D
{
    public class CollisionWorld
    {
        public IBroadphase BroadPhase;

        public List<Contact> ContactList = new List<Contact>(128);//in use
        internal Queue<Contact> _contactPool = new Queue<Contact>(256);//recycle
        public Action OnPreCollision;
        public Action OnPostCollision;

        public CollisionWorld(IBroadphase broadPhase)
        {
            BroadPhase = broadPhase;
        }

        //user add new fixture to world
        public void AddFixture(Fixture fixture)
        {
            int proxyId = BroadPhase.AddProxy(fixture.Proxy);
            fixture._world = this;
            fixture.OnAddToWorld(this);
        }

        //user remove fixture from world
        public void RemoveFixture(Fixture fixture)
        {
            //去掉所有的contact
            while(fixture.ContactList != null)
            {
                Destroy(fixture.ContactList.Contact);
            }

            BroadPhase.RemoveProxy(fixture.Proxy.ProxyId);
            fixture.OnRemoveFromWorld(this);
            fixture._world = null;
        }

        // Broad-phase callback.
        private void AddPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
        {
            Fixture fixtureA = proxyA.Fixture;
            Fixture fixtureB = proxyB.Fixture;

            if (fixtureA == fixtureB)
            {
                return;
            }

            // Does a contact already exist?
            ContactEdge edge = fixtureB.ContactList;
            while (edge != null)
            {
                if (edge.Other == fixtureA)
                {
                    Fixture fA = edge.Contact.FixtureA;
                    Fixture fB = edge.Contact.FixtureB;

                    if (fA == fixtureA && fB == fixtureB)
                    {
                        // A contact already exists.
                        return;
                    }

                    if (fA == fixtureB && fB == fixtureA)
                    {
                        // A contact already exists.
                        return;
                    }
                }

                edge = edge.Next;
            }

            //Check default filter
            if (ShouldCollide(fixtureA, fixtureB) == false)
                return;

            // Call the factory.
            Contact c = Contact.Create(fixtureA, fixtureB);

            if (c == null)
                return;

            // Insert into the world.
            ContactList.Add(c);

            // Connect to body A
            c._nodeA.Contact = c;
            c._nodeA.Other = fixtureB;

            c._nodeA.Prev = null;
            c._nodeA.Next = fixtureA.ContactList;
            if (fixtureA.ContactList != null)
            {
                fixtureA.ContactList.Prev = c._nodeA;
            }
            fixtureA.ContactList = c._nodeA;

            // Connect to body B
            c._nodeB.Contact = c;
            c._nodeB.Other = fixtureA;

            c._nodeB.Prev = null;
            c._nodeB.Next = fixtureB.ContactList;
            if (fixtureB.ContactList != null)
            {
                fixtureB.ContactList.Prev = c._nodeB;
            }
            fixtureB.ContactList = c._nodeB;

        }

        internal void Destroy(Contact contact)
        {
            Fixture fixtureA = contact.FixtureA;
            Fixture fixtureB = contact.FixtureB;

            // Remove from the world.
            ContactList.Remove(contact);

            // Remove from body 1
            if (contact._nodeA.Prev != null)
            {
                contact._nodeA.Prev.Next = contact._nodeA.Next;
            }

            if (contact._nodeA.Next != null)
            {
                contact._nodeA.Next.Prev = contact._nodeA.Prev;
            }

            if (contact._nodeA == fixtureA.ContactList)
            {
                fixtureA.ContactList = contact._nodeA.Next;
            }

            // Remove from body 2
            if (contact._nodeB.Prev != null)
            {
                contact._nodeB.Prev.Next = contact._nodeB.Next;
            }

            if (contact._nodeB.Next != null)
            {
                contact._nodeB.Next.Prev = contact._nodeB.Prev;
            }

            if (contact._nodeB == fixtureB.ContactList)
            {
                fixtureB.ContactList = contact._nodeB.Next;
            }

            contact.Destroy();
        }

        private static bool ShouldCollide(Fixture fixtureA, Fixture fixtureB)
        {
            int minCollisionCategory = (int)(fixtureA.CollisionCategories < fixtureB.CollisionCategories ? fixtureA.CollisionCategories : fixtureB.CollisionCategories);
            int maxCollisionCategory = (int)(fixtureA.CollisionCategories > fixtureB.CollisionCategories ? fixtureA.CollisionCategories : fixtureB.CollisionCategories);
            return (fixtureA.Enabled && fixtureB.Enabled) && Settings.CollisionMatrix[minCollisionCategory, maxCollisionCategory]; 
        }

        internal void UpdateAABB(Fixture fixture)
        {
            BroadPhase.MoveProxy(fixture.Proxy.ProxyId, fixture.GetAABB());
        }

        public void Collide()
        {
            BroadPhase.UpdatePairs(AddPair);

            if (OnPreCollision != null)
                OnPreCollision.Invoke();

            for (int i = 0; i < ContactList.Count; i++)
            {
                Contact c = ContactList[i];
                Fixture fixtureA = c.FixtureA;
                Fixture fixtureB = c.FixtureB;

                // Check default filtering
                if (ShouldCollide(fixtureA, fixtureB) == false)
                {
                    Contact cNuke = c;
                    Destroy(cNuke);
                    continue;
                }

                int proxyIdA = fixtureA.Proxy.ProxyId;
                int proxyIdB = fixtureB.Proxy.ProxyId;

                bool overlap = BroadPhase.TestOverlap(proxyIdA, proxyIdB);

                // Here we destroy contacts that cease to overlap in the broad-phase.
                if (overlap == false)
                {
                    Contact cNuke = c;
                    Destroy(cNuke);
                    continue;
                }

                // The contact persists.
                c.Update();
            }

            if (OnPostCollision != null)
                OnPostCollision.Invoke();
        }
    }
}


